#pragma once

// Eigen
#include "Eigen/Core"
#include "Eigen/Dense"

// Sophus
#include "sophus/se3.hpp"
#include "sophus/so3.hpp"

namespace vio {
namespace math_utils {

template <typename T>
T GetMedian(std::vector<T>& data_vec) {
  assert(!data_vec.empty());
  typename std::vector<T>::iterator it =
      data_vec.begin() + std::floor(data_vec.size() / 2);
  std::nth_element(data_vec.begin(), it, data_vec.end());
  return *it;
}

inline Eigen::Vector2d project2d(const Eigen::Vector3d& v) {
  return v.head<2>() / v[2];
}

inline Eigen::Vector3d unproject2d(const Eigen::Vector2d& v) {
  return Eigen::Vector3d(v[0], v[1], 1.0);
}

inline Eigen::Vector3d project3d(const Eigen::Vector4d& v) {
  return v.head<3>() / v[3];
}

inline Eigen::Vector4d unproject3d(const Eigen::Vector3d& v) {
  return Eigen::Vector4d(v[0], v[1], v[2], 1.0);
}

inline Eigen::Matrix3d sqew(const Eigen::Vector3d& v) {
  Eigen::Matrix3d v_sqew;
  v_sqew << 0, -v[2], v[1], v[2], 0, -v[0], -v[1], v[0], 0;
  return v_sqew;
}

double SampsonusError(const Eigen::Vector2d& v2Dash,
                      const Eigen::Matrix3d& Essential,
                      const Eigen::Vector2d& v2);

Eigen::Vector3d TriangulateFeatureNonLin(const Eigen::Matrix3d& R,
                                         const Eigen::Vector3d& t,
                                         const Eigen::Vector3d& feature1,
                                         const Eigen::Vector3d& feature2);

double ReprojError(const Eigen::Vector3d& f1,
                   const Eigen::Vector3d& f2,
                   double error_multplier2);

double ComputeInliers(const std::vector<Eigen::Vector3d>& features1,
                      const std::vector<Eigen::Vector3d>& features2,
                      const Eigen::Matrix3d& R,
                      const Eigen::Vector3d& t,
                      const double reproj_thresh,
                      double error_multiplier2,
                      std::vector<Eigen::Vector3d>& xyz_vec,
                      std::vector<int>& inliers,
                      std::vector<int>& outliers);
}  // namespace math_utils
}  // namespace vio
